#include <stdlib.h>
#include <fcntl.h>
#include <unistd.h>
#include <stdio.h>
#include <sys/time.h>
#include <unistd.h>
#include <fstream>
#include <iomanip>
#include <ctime>
#include "../include/serial.h"
#include "../include/timeout.h"
#include <iostream>
using namespace std;

// 线程体，不断地读取数据
void *ReadFunction(void *arg)
{
    std::cout<<"ReadFunction start";
    serial serial;
    char buffer[256] = {0};
    int i, length = 0;

    // Open serial port ("COM3", "/dev/ttyUSB0")
    char* ch = (char*)"/dev/ttyS1";
    std::cout<<ch<<std::endl;
    serial.Open(ch, 115200, 8, NO, 1);
    int count = 100;
    while(count)
    {
        // Wait character
        length = serial.Read(buffer);
        if(length)
        {
            for(i = 0; i < length; i++)
            {
                printf("%.2X ", buffer[i]);
                
            }//if buffer equal to imu_strRecv
             //then the subarry member will print AA 44
             //@test
            std::cout<<hex<<(int)buffer[0]<< "=="<<"0xAA ??"<<std::endl;
            std::cout<<hex<<(int)buffer[1]<< "=="<<"0x44 ??"<<std::endl;
            // Send data
            serial.Write(buffer, length);
        }
        count--;
    }
    // Close serial port
    serial.Close();
}
	
parse_data(std::vector<u_int8_t> strRecv)

{
	imu_strRecv.insert(imu_strRecv.end(), strRecv.begin(), strRecv.end());//居然用的insert，在末尾插入
	cout << "rec size:"<<imu_strRecv.size()<<endl;//整体size（）,不一定是104
	while (imu_strRecv.size() >=104) //至少包含帧头（2字节）、长度（1字节）、功能位（1字节）；根据设计不同而不同
	{
		cout<<"104";
		// 2.1 查找数据头
		u_int64_t Milliseconds=imu_strRecv[6];
		if (imu_strRecv[0] == 0xAA &&imu_strRecv[1] == 0x44) //传输数据有帧头，用于判断. 找到帧头  AA AA 0A
		{

			ushort ID = 0;
			memcpy(&ID, imu_strRecv.data() + 4, 2);
			short len = 0;
			if (ID == 508)
			{
                cout<<"508";//508才会写入文件
                unsigned char length = (unsigned char)imu_strRecv[3];
                len = (short)length + 16;
                if (imu_strRecv.size() < len) //数据区尚未接收完整，
                {
                break; //跳出接收函数后之后继续接收数据
                }
                //执行其他代码，对数据进行处理。
                //解析5 6， 7 8字节的经纬度.
                uint32_t week;
                memcpy(&CPT_imu, imu_strRecv.data() + 16, length);
                imu_strRecv.erase(imu_strRecv.begin(), imu_strRecv.begin() + len);
                cout<<"i will write the imu data"<<endl;
                cout<<"508 CPT_imu.Z_Accuracy is "<<CPT_imu.Z_Accuracy<<endl;
		    }
			else if (ID == 1465)
			{
				len = (imu_strRecv[9] << 8) + imu_strRecv[8] + 32;
				if (imu_strRecv.size() < len) //数据区尚未接收完整，
				{
					break; //跳出接收函数后之后继续接收数据
				}
				float azimuthdev = 0;
				memcpy(&azimuthdev, imu_strRecv.data() + 28 + 116, 4);
				imu_strRecv.erase(imu_strRecv.begin(), imu_strRecv.begin() + len);
				CPT_imu.Z_Accuracy = azimuthdev;
                cout<<"i will write the imu data"<<endl;
                cout<<"1465 CPT_imu.Z_Accuracy is "<<CPT_imu.Z_Accuracy<<endl;
			}
		}else {
            //帧头长度不正确时，清除
			imu_strRecv.erase(imu_strRecv.begin(), imu_strRecv.begin() + 1); //为啥begin()+1,这个1的实际长度多少
		}
	}
}